#ifndef _SLAM_H_
#define _SLAM_H_

#include "rads_base.hpp"
#include <iostream>
#include <sstream>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <cstring>
#include <vector>
#include <thread>
#include <cmath>
#include <chrono>
#include <ctime>
#include <condition_variable>

#include <eigen3/Eigen/Dense>

#include <ros/ros.h>
#include <ros/time.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <rads/rads_trackext.h>
#include <std_msgs/Float32.h>
#include <tf/transform_listener.h>
class LidarRtkFusion
{
public:
    rads::rads_trackext fusion_data;
public:
    LidarRtkFusion() : lidar_received_(false), rtk_received_(false),ndt_received_(false) {}
    void lidarCallback(const geometry_msgs::PoseStamped& msg)
    {
        lidar_data_ = msg;
        lidar_received_ = true;
        checkAndProcessData();
    }
    void rtkCallback(const rads::rads_trackext& nav)
    {
        rtk_data_ = nav;
        rtk_received_ = true;
        checkAndProcessData();
    }
    void ndtCallback(const std_msgs::Float32::ConstPtr& msg)
    {
        ndt_data_ = msg;
        ndt_received_ = true;
        checkAndProcessData();
    }
private:
    void checkAndProcessData()
    {
        // 这里仅仅是测试,目前这里简单处理
        if (lidar_received_)
        {
           
            fusion_data.x = lidar_data_.pose.position.x;
            fusion_data.y = lidar_data_.pose.position.y;
            fusion_data.z = lidar_data_.pose.position.z;
            fusion_data.ox = lidar_data_.pose.orientation.x;
            fusion_data.oy = lidar_data_.pose.orientation.y;
            fusion_data.oz = lidar_data_.pose.orientation.z;
            fusion_data.ow = lidar_data_.pose.orientation.w;
			lidar_received_ = false;
        }
        
		if (rtk_received_)
        {
            fusion_data.rtk_x  =rtk_data_.x;
            fusion_data.rtk_y  =rtk_data_.y;
            fusion_data.rtk_z = rtk_data_.z;
            fusion_data.yaw = rtk_data_.yaw;
            fusion_data.longitude = rtk_data_.longitude;
            fusion_data.latitude = rtk_data_.latitude;
            fusion_data.altitude = rtk_data_.altitude;
            fusion_data.heading = rtk_data_.heading;
            fusion_data.posi_flag = rtk_data_.posi_flag;
            fusion_data.head_flag = rtk_data_.head_flag;
            fusion_data.groud_speed = rtk_data_.groud_speed;
            fusion_data.ext = rtk_data_.ext;
            fusion_data.flag = rtk_data_.flag;
			rtk_received_ = false;
        }
        if (ndt_received_)
        {
            fusion_data.matching_rate = ndt_data_->data*10;
			ndt_received_ = false;
        }
    }
    // 激光匹配POSE
    geometry_msgs::PoseStamped lidar_data_;
    std_msgs::Float32ConstPtr ndt_data_;
    // 传统GNSS数据
    rads::rads_trackext rtk_data_;
    bool lidar_received_;
    bool rtk_received_;
    bool ndt_received_;
};

/*
            Eigen::Quaterniond quaternion(lidar_data_.orientation.w, lidar_data_.orientation.x, lidar_data_.orientation.y, lidar_data_.orientation.z);
            Eigen::Matrix3d rotationMatrix = quaternion.matrix();
            // 计算pitch角（检查pitch是否接近±90°）
            double pitchRad = asin(2 * (rotationMatrix(0, 2) - rotationMatrix(2, 0)));
            double pitchDeg = pitchRad * 180.0 / PI;
            double yawDeg,rollDeg;
            // 检查万向锁
            if (fabs(pitchDeg) >= 89.999) {
                ROS_WARN("Euler angles may be inaccurate due to gimbal lock (pitch close to ±90°).");
                yawDeg = std::numeric_limits<double>::quiet_NaN();
                rollDeg = std::numeric_limits<double>::quiet_NaN();
            } else {
                // 从旋转矩阵中提取欧拉角（ZYX顺序，即yaw-pitch-roll）
                yawDeg = atan2(2 * (rotationMatrix(0, 1) - rotationMatrix(1, 0)),1 - 2 * (rotationMatrix(0, 0) - rotationMatrix(1, 1))) * 180.0 / PI;
                rollDeg = atan2(2 * (rotationMatrix(1, 2) - rotationMatrix(2, 1)), 1 - 2 * (rotationMatrix(1, 1) - rotationMatrix(2, 2))) * 180.0 / PI;
            }
            #ifdef DEBUG
            ROS_INFO("x = %f,y = %f,z = %f",lidar_data_.position.x,lidar_data_.position.y,lidar_data_.position.z);
            ROS_INFO("yaw = %f,pitch = %f,roll = %f", yawDeg,pitchDeg,rollDeg);
            ROS_INFO("latitude = %f,longitude = %f,altitude = %f",rtk_data_.latitude,rtk_data_.longitude,rtk_data_.altitude);
            #endif
*/
#endif
